
#include "Components/algorithm.h"
#include "Components/basic.h"
#include "Components/line_patrol.h"
#include "Components/motion.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/servo.h"
#include <ti/driverlib/m0p/dl_interrupt.h>

#include <math.h>

#define TAG "Main"

#define Vertical_Servo_Mode BUS_SERVO_MODE_ANTI_CLOCKWISE_180
#define Horizontal_Servo_Mode BUS_SERVO_MODE_ANTI_CLOCKWISE_180
#define Vertical_Servo_ID 1
#define Horizontal_Servo_ID 0

#define H 10
#define PanTilt_K 1.0
#define PI (3.14159)

#define Vertical_Servo_Min_PWM (1200)

typedef struct
{
  int16_t err_x;
  int16_t err_y;
} recv_data_t;

void panTilt_set_angle(uint16_t hor, uint16_t ver)
{
#define _GET_180_Pwm(angle) (500 + angle * 2000 / 180)
#define Vertical_PWM_Check(pwm) \
  (pwm <= Vertical_Servo_Min_PWM ? Vertical_Servo_Min_PWM : pwm)

  bus_servo_set_pwm(Horizontal_Servo_ID, _GET_180_Pwm(hor), 0);
  bus_servo_set_pwm(Vertical_Servo_ID, Vertical_PWM_Check(_GET_180_Pwm(ver)),
                    0);
}
void panTilt_init()
{
  bus_servo_set_mode(Horizontal_Servo_ID, Horizontal_Servo_Mode);
  delay(100);
  bus_servo_set_mode(Vertical_Servo_ID, Vertical_Servo_Mode);
  delay(100);
  panTilt_set_angle(0, 0);
}

uint16_t panTilt_calculate_angle(int16_t _err, int16_t _cur_angle)
{
  float err = PanTilt_K * _err;
  float _angle;
  float cur_angle = _cur_angle * PI / 180.0;

  if (_cur_angle != 90)
  {
    if (fabs(H + err * tan(cur_angle)) > 1e-6)
      _angle = atan((H * tan(cur_angle)) / (H + err * tan(cur_angle)));
    else
      return 90;
  }
  else
    _angle = atan(H / err);
  if (_angle < 0)
    _angle = PI + _angle;

  uint16_t angle = (int)round(_angle * 180.0 / PI);
  return angle;

  int pulse_width = (int)round(angle * (2000.0 / 180.0) + 500);
  return pulse_width;
}

void parseUartData(recv_data_t *recv_data, uint8_t *serial_data, uint16_t len)
{
  const uint8_t pack_head[2] = {0xa5, 0xa6};
  uint8_t *s = serial_data;

  for (uint16_t i = 0; i < len - 1; i++, s++)
  {
    if (*s == pack_head[0] && *(s + 1) == pack_head[1])
    {
      break;
    }
  }

  if (s == serial_data + len - 1)
  {
    recv_data->err_x = 0;
    recv_data->err_y = 0;
    return;
  }

  s += 2;
  recv_data->err_x = (*s ? 1 : -1) * (*(s + 1));
  s += 2;
  recv_data->err_y = (*s ? 1 : -1) * (*(s + 1));
}

float delta_seconds = 0;

void get_delta_seconds()
{
  static uint32_t last_ms = 0;
  uint32_t cur_ms;
  mspm0_get_clock_ms(&cur_ms);

  delta_seconds = (cur_ms - last_ms) / 1000.0f;
  last_ms = cur_ms;
}
int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();
  uart_init();

  panTilt_init();

  int16_t cur_hor_angle = 45;
  int16_t cur_ver_angle = 45;
  pid_control_t pid_hor = {0}, pid_ver = {0};

  pid_init(&pid_hor, 0.2, 0, 0, 10);
  pid_init(&pid_ver, 0.2, 0, 0, 10);
  log(TAG, "Init Done!");

  log(TAG, "Init Done!");

  // test

  // my_printf("test 1 %d\n", panTilt_calculate_angle(-100, 45));
  // my_printf("test 1 %d\n", panTilt_calculate_angle(-200, 45));

  for (;;)
  {

    uint8_t *str = NULL;

    uint16_t str_len = getCurrentUart1Data(&str);
    if (str_len && str)
    {
      setCurrentUART1DataLock(true);

      recv_data_t recv_data;
      parseUartData(&recv_data, str, str_len);
      get_delta_seconds();
      cur_hor_angle += pid_update(&pid_hor, recv_data.err_x, delta_seconds);
      cur_ver_angle += pid_update(&pid_ver, recv_data.err_y, delta_seconds);

      if (cur_hor_angle < 0)
        cur_hor_angle = -cur_hor_angle;
      if (cur_ver_angle < 0)
        cur_ver_angle = -cur_ver_angle;

      panTilt_set_angle(cur_hor_angle, cur_ver_angle);

      log(TAG, "cur_hor_angle: %d, cur_ver_angle: %d", cur_hor_angle,
             cur_ver_angle);
      log(TAG, "recv_data: err_x: %d, err_y: %d", recv_data.err_x,
             recv_data.err_y);

      setCurrentUART1DataLock(false);
    }

    //   #define TEST_CASE_NUM 2
    // uint16_t test_case[TEST_CASE_NUM][2] = {
    //     {0, 180},
    //     {180, 0},
    // };

    // for (int i = 0; i < TEST_CASE_NUM; i++)
    // {
    //   for (int j = 0; j < 2; j++)
    //   {
    //     panTilt_set_angle(test_case[i][0], test_case[i][1]);
    //     delay(500);
    //   }
    // }
  }
}
